"""
src/ode3d/linact.py
    A library of PyODE3D objects / assemblies.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009-2012

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 3 of
the License, or (at your option) any later version. The text of the
GNU Lesser General Public License is included with this library in
the file named LICENSE.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
"""
from time import time as timeNow

from direct.gui.OnscreenText import OnscreenText
from pandac.PandaModules import ClockObject, Vec4, Vec3
from pandac.PandaModules import AmbientLight,Spotlight

import gdm
import world
import odelib
import pvis

class Piston(gdm.GDMAssembly):
    def __init__(self, World, Length, Diameter, MinSpringForce=0, MaxSpringForce=0):
        """Create a visualized pistion with spring, damping, and actuation properties.
        World:         The pyode world that this object resides in
        Length:     The fully extended length of the pistion
        Diameter:     The diameter of the outer cylinder
        MinSpringForce: The spring force applied at the minimum length position
        MaxSpringForce: The spring force applied at the maximum length position
                Note:    A positive spring force makes the pistons expand
                        A negative spring force makes the pistons contract
        """

        # Initialize the ode3d.GDMAssembly class
        gdm.GDMAssembly.__init__(self)

        self._length = Length # We might want to keep this around

        cylLength = Length / 2.0 # The le3ngth of each cylinder is half of the total.

        # Create the two halves of the piston -- Build it collapsed

        innerCylBody = gdm.GDMBody(World)
        innerCylElement = gdm.GDMElement().DefineCylinder(2000, 0.75 * Diameter, cylLength)
        innerCylBody.AddElement('cyl', innerCylElement)
        innerCylBody.DefineConnectionPoint('Tip', (0, 0, cylLength / 2.0))
        innerCylBody.DefineConnectionPoint('Slider', (0, 0, 0), pvis.zAxis)

        outerCylBody = gdm.GDMBody(World)
        outerCylElement = gdm.GDMElement().DefineCylinder(2000, Diameter, cylLength)
        outerCylBody.AddElement('cyl', outerCylElement)
        outerCylBody.DefineConnectionPoint('Tip', (0, 0, -cylLength / 2.0))
        outerCylBody.DefineConnectionPoint('Slider', (0, 0, 0), pvis.zAxis)

        self._linearActuatorJoint = odelib.LinearActuator(World)

        # This order gives us positive slider positions whith fully extended being cylLength
        innerCylBody.Connect(self._linearActuatorJoint, 'Slider', outerCylBody, 'Slider')

        # Add these bodies to the assembly
        self.AddObj('Inner', innerCylBody)
        self.AddObj('Outer', outerCylBody)

        # MinSpringForce is the force you get at MinStop
        # MaxSpringForce is the force you get at MaxStop
        # A positive spring force makes the pistons expand
        # A negative spring force makes the pistons contract
        self._linearActuatorJoint.ChangeProperties(MinSpringForce=MinSpringForce,
                    MaxSpringForce=MaxSpringForce, MinStop=0, MaxStop=cylLength)

    def GetInnerConnectionInfo(self):
        """Get the (body, ID) information for the connection on the inner cylinder's end."""
        return (self.GetObj('Inner'), 'Tip')

    def GetInnerConnectionLocation(self):
        """Get the location of the connection point on the inner cylinder's end."""
        return self.GetObj('Inner').GetConnectionLocation('Tip')

    def GetOuterConnectionInfo(self):
        """Get the (body, ID) information for the connection on the outer cylinder's end."""
        return (self.GetObj('Outer'), 'Tip')

    def GetOuterConnectionLocation(self):
        """Get the location of the connection point on the outer cylinder's end."""
        return self.GetObj('Outer').GetConnectionLocation('Tip')
    

class UserHandler(object):
    def __init__(self, App):
        self._app = App
        
        self.simState = odelib.SimState()

        self._clickedAssembly = None

        self._dragging = False
        self._dragPos = None
        self._dragLast = None

        self._singleStep = False
        self._stepTime = False

        self.kbHelp = [('t', 'Toggle Stepping'), ('s', 'Step')]
        
        #self._helpDisplay = vis3d.label(pos=(5, Screen.height - 5, 0), font='Sans', height=13)

    @property
    def app(self):
        return self._app

    @property
    def clickedAssembly(self):
        return self._clickedAssembly

    @property
    def dragging(self):
        return self._dragging
    
    @property
    def dragPos(self):
        return self._dragPos
    
    @property
    def dragLast(self):
        return self._dragLast
    
    @property
    def singleStep(self):
        if self._singleStep:
            # Toggle back to false automatically
            self._singleStep = False
            return True
        else:
            return False
    
    @property
    def stepTime(self):
        return self._stepTime
    @stepTime.setter
    def stepTime(self, Value):
        self._stepTime = bool(Value)
        
    def ProcChar(self, KeyChar):
        if KeyChar == 's':
            self._singleStep = True
        elif KeyChar == 't':
            # Toggle time stepping mode
            # when stepTime is True, we will run one time
            # step each time singleStep is set True
            self._stepTime = not self._stepTime

    def ProcMouse(self):
        # The mouse data has been updated, we can do something
        # Here if needed.
        pass

    def UpdateSettings(self):
        pass
    
    def UpdateDisplay(self):
        pass
    
class OdeApp(object):
    def __init__(self, TimeStep, FrameRate=None, FTFactor=1, Gravity=(0,0,-9.81), 
                 UserHandlerClass=None, UseQuickStep=False,
                 Display=None, Size=(640, 480), Title='Ode3d App'):
        self._frameNum = 0
        self._simTime = 0
        self._dt = TimeStep
        self._frameRate = FrameRate
        self._simStepsPerFrame = 1
        self.simState = odelib.SimState()
        self._running = True
        
        if self._frameRate is not None:
            self._frameRate = FrameRate
            
            self._simStepsPerFrame = max(int(1.0 / self._frameRate / self._dt + 0.5), 1)
            self._dt = 1.0 / self._frameRate / self._simStepsPerFrame
            self._globalClock = ClockObject.getGlobalClock()
            self._globalClock.setMode(ClockObject.MLimited)
            self._globalClock.setFrameRate(self._frameRate * FTFactor)

        self.simState.timeStep = self.dt

        print 'TimeStep=%f, FrameRate=%s' % (TimeStep, str(FrameRate))
        print 'dt=%f, frameRate=%s, stepsPerframe=%s' % (self._dt,
                                                         str(self._frameRate),
                                                         self._simStepsPerFrame)
        self._world = world.World(Display=Display, Size=Size, Title=Title)
        self._world.setGravity(Gravity)
        #self._world.setERP(0.8)
        #self._world.setCFM(1E-5)
        self._stepFunc = self._world.quickStep if UseQuickStep else self._world.step

        self._objectsDict = {}
        
        if UserHandlerClass is not None:
            self._userHandler = UserHandlerClass(self)
        else:
            self._userHandler = UserHandler(self)
        

        self._statusDisplay = OnscreenText(pos = (-0.95, -.9), scale = 0.07)
        self._statusDisplay.visible = True
        
    @property
    def dt(self):
        return self._dt
    
    @property
    def framerate(self):
        return self._frameRate
    
    @property
    def userHandler(self):
        return self._userHandler

    @property
    def simTime(self):
        return self._simTime
    
    @property
    def toggleStatus(self):
        self._statusDisplay.visible = not self._statusDisplay.visible
        
        return self._statusDisplay.visible
    
    @property
    def stepTime(self):
        return self._userHandler.stepTime
    @stepTime.setter
    def stepTime(self, Value):
        self._userHandler.stepTime = Value

    @property
    def world(self):
        return self._world
        
    def AddObj(self, Name, Obj):
        self._objectsDict[Name] = Obj
        
    def GetObj(self, Name):
        return self._objectsDict.get(Name, None)

    def ProcKB(self):
        return
        # Check for a character input from the keyboard
        if self._world.KBHit():
            s = self._world.GetKey()
            char = s[0] # TODO: Need to handle shift-states and special characters

            self.userHandler.ProcChar(char)

    def ProcMouse(self):
        return
        scene = self._world.scene
        if scene.mouse.events:
            mouseEvent = scene.mouse.getevent() # obtain click, drag or drop event
            pickedObj = mouseEvent.pick
            pickedPos = mouseEvent.pickpos
    
            if mouseEvent.drag and pickedObj:
                # Save information for dragging
                self.userHandler._dragging = True
                self.userHandler._dragPos = pickedPos
                self.userHandler._dragLast = pickedPos
    
            elif mouseEvent.drop and self.userHandler.dragging:
                # This is the end of a drag
                self.userHandler._dragging = False
                self.userHandler._drop = True
                self.userHandler._dropPos = pickedPos
                
            elif mouseEvent.click:
                self._userHandler._clickedObj
                # See if the user clicked on an object in the scene
                for obj in self._objectsDict.itervalues():
                    if obj == pickedObj:
                        self._userHandler._clickedObj = obj
                        break # Stop after the first one, can there be multiple?

        # Mouse Movements matter if we are dragging
        mousePos = scene.mouse.pos
        if self.userHandler.dragging and (mousePos != self.userHandler.dragLast):
            # update drag position
            self.userHandler._dragLast = mousePos

        self.userHandler.ProcMouse()
    
    def UpdateSettings(self):
        # Allow a user function to update settings if desired
        self.userHandler.UpdateSettings()
        
        # Update settings for all our assemblies
        for obj in self._objectsDict.itervalues():
            obj.UpdateSettings()
        
    def UpdateDisplay(self):
        self._statusDisplay.setText('SimTime: %0.3f\nFrame #: %d' % (self.simState.simTime, self._frameNum))

        for obj in self._objectsDict.itervalues():
            #Update the display for each obj
            obj.UpdateDisplay()

        # Allow the user handler to make updates
        self.userHandler.UpdateDisplay()

    def dump(self):
        for obj in self._objectsDict.itervalues():
            try:
                obj.dump()
                print
            except AttributeError:
                pass

    def UpdateLoop(self):
        # Allow for single stepping one frame at a time
        if (not self.userHandler.stepTime) or self.userHandler.singleStep:
            self._frameNum += 1

            # Run multiple sim steps per frame update as needed
            for i in xrange(self._simStepsPerFrame):
                self._DoSimStep()
                
        self.ProcKB()

        self.ProcMouse()

        self.UpdateDisplay()

    def _DoSimStep(self):
        if self._simTime >= self._printTime:
            try:
                factor = (self._simTime - self._startTime) / (timeNow() - self._startRealTime)
            except ZeroDivisionError:
                factor = 1.0
                
            #print 'SimTime: % 9.2f  % 6.2fx Real-Time' % (self._simTime, factor)
            self._printTime += 1000 * self.dt # Print every 1k sim steps
        
        self.UpdateSettings()
        
        # Run one time step on the physics engine
        self._stepFunc(self.dt)
        self._simTime += self.dt

        self.simState.simTime = self._simTime
        self.simState.simLoop += 1
        
    def Run(self, StartTime=None, Static=False):
        self._startRealTime = timeNow()
        self._simTime = self._startRealTime if StartTime is None else StartTime
        self._startTime = self._simTime
        self._printTime = self._simTime

        if self._frameRate is None:
            while True:
                self._DoSimStep()

        elif Static:
            self.UpdateSettings()
            self.UpdateDisplay()
            #from direct.showbase.DirectObject import DirectObject
            #direct = DirectObject()
            #direct.Run()
            pvis.Run()
            
        else:
            def update(task):
                self.UpdateLoop()
                if self._running:
                    return task.cont
                else:
                    return task.done
            
            #taskMgr.add(update, 'Update Loop')

            pvis.Run(update)